#include "load.h"
#include "functions.h"
#include <stdlib.h>
#include <pcl/io/pcd_io.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/angles.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/impl/kdtree_flann.hpp>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/passthrough.h>

int loadCloud(string path, PointCloud<PointXYZ>::Ptr cloud,float leafSize,bool segmentPlane,bool removeOut)
{
    int millistart;
    int span;
    millistart= getMilliCount();
    cout << "Loading cloud..."<<flush;
    io::loadPCDFile(path,*cloud);
    span =getMilliSpan(millistart);
    cout<<"Loading from HDD milliseconds: *******************************************************************************"<<span<<endl;
    cout << " Original: " << cloud->size();
        millistart= getMilliCount();
    if(leafSize>0) voxel (cloud,leafSize);
    cout << " Voxelled: " << cloud->size();
    if(segmentPlane) segmentMainPlane(cloud,4.0);
    cout << " WithoutPlane: " << cloud->size();
    if (removeOut) removeOutliers(cloud,cloud,2,1);
    //if (removeOut) removeOutliers(cloud,cloud,5,1);//2nd pass
    cout << " Cleaned: " << cloud->size()<<endl;
    span =getMilliSpan(millistart);
    cout<<"Preprocessing milliseconds: *******************************************************************************"<<span<<endl;
}

void voxel(PointCloud<PointXYZ>::Ptr cloud, float leafSize)
{
    //cout << "Voxelling "<< cloud->size() <<" points... "<<flush;
    VoxelGrid<PointXYZ> vg;
    vg.setInputCloud (cloud);
    vg.setLeafSize (leafSize,leafSize,leafSize);
    vg.filter (*cloud);
    //cout << cloud->points.size() << " Voxels\n";
}

void segmentMainPlane(PointCloud<PointXYZ>::Ptr cloud, double planeThreshold)
{
    //cout << "Main Plane Segmentation... " << flush;
    ModelCoefficients::Ptr coefficients (new ModelCoefficients);
    PointIndices::Ptr inliers (new PointIndices);
    SACSegmentation<PointXYZ> seg;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (SACMODEL_PERPENDICULAR_PLANE);//(SACMODEL_PLANE);
    seg.setAxis (Eigen::Vector3f (0.0, 0.0, 1.0));//parallelo asse z
    seg.setEpsAngle(deg2rad(5.0f));
    seg.setMethodType (SAC_RANSAC);
    seg.setDistanceThreshold (planeThreshold);
    seg.setInputCloud (cloud);
    seg.segment (*inliers, *coefficients);
    ExtractIndices<PointXYZ> extract;
    extract.setInputCloud (cloud);
    extract.setIndices (inliers);
    extract.setNegative (true);
    extract.filter (*cloud);
    vector<int> indices;
    removeNaNFromPointCloud(*cloud, *cloud, indices);
    //cout << "OK! Segmented: now there are " << cloud->size  () << " points.\n";


}


void removeOutliers(PointCloud<PointXYZ>::Ptr source,PointCloud<PointXYZ>::Ptr target, int meanK, double stdDev)
{
    //cout << "Removing outliers... " << flush;
    StatisticalOutlierRemoval<PointXYZ> sor;
    sor.setInputCloud (source);
    sor.setMeanK (meanK);//2 Set the number of nearest neighbors to use for mean distance estimation.
    sor.setStddevMulThresh (stdDev);//1 Set the standard deviation multiplier for the distance threshold calculation.
    sor.filter (*target);
    //cout << "OK! " << cloud->size() << " points Loaded."<<endl;
}

void removeOutliers(PointCloud<PointXYZI>::Ptr source,PointCloud<PointXYZI>::Ptr target, int meanK, double stdDev)
{
    //cout << "Removing outliers... " << flush;
    StatisticalOutlierRemoval<PointXYZI> sor;
    sor.setInputCloud (source);
    sor.setMeanK (meanK);//2 Set the number of nearest neighbors to use for mean distance estimation.
    sor.setStddevMulThresh (stdDev);//1 Set the standard deviation multiplier for the distance threshold calculation.
    sor.filter (*target);
    //cout << "OK! " << cloud->size() << " points Loaded."<<endl;
}
